﻿using System.Collections;

namespace WayPointNavigator
{
    public class CircularQueue : Queue
    {
        public new GpsPoint Dequeue()
        {
            GpsPoint tmp = (GpsPoint)base.Dequeue();
            Enqueue(tmp);
            return tmp;
        }

        public void CycleTo(GpsPoint item)
        {
            // Using a for loop here to avoid and endless cycle if no match is found!
            GpsPoint tmp = Peek() as GpsPoint;
            for (int i = 0; i < Count; i++)
            {
                if (!tmp.Equals(item))
                {
                    // Toss it to the back.
                    Dequeue();
                    // Check the next one.
                    tmp = Peek() as GpsPoint;
                }
                else
                {
                    // We've hit our item. Stop here!
                    return;
                }
            }
            // No item was found, so make sure we end up where we started from.
            Dequeue();
        }

        public GpsPoint GetClosestToMe(GpsPoint MeLocation)
        {
            // Store my location.
            GpsPoint myLoc = MeLocation;

            // Since we want the closest, we start with the highest possible value
            // to make sure we have some valid data!
            double closest = double.MaxValue;

            var ret = new GpsPoint();

            // Cycle through, testing distance, and set our return Point to the closest
            // one we find.
            foreach (GpsPoint p in this)
            {
                if (p.Distance(myLoc) < closest)
                {
                    closest = p.Distance(myLoc);
                    ret = p;
                }
            }
            return ret;
        }
    }
}